function particles = mcl_update(particles, scan, occ_map, max_range, fov, num_beams, resolution, map_size)
% 蒙特卡洛定位更新步骤
% 输入: particles-粒子, scan-激光扫描, occ_map-占据栅格
% 输出: particles-更新后粒子

    N = length(particles);
    grid_size = size(occ_map, 1);
    
    for i = 1:N
        pose = particles(i).pose;
        
        % 模拟该粒子位置的预期扫描
        expected_scan = simulate_scan_from_map(pose, occ_map, max_range, fov, num_beams, resolution, map_size);
        
        % 计算似然（扫描匹配度）
        likelihood = compute_scan_likelihood(scan, expected_scan);
        
        % 更新权重
        particles(i).weight = particles(i).weight * likelihood;
    end
    
    % 归一化权重
    total_weight = sum([particles.weight]);
    if total_weight > 1e-10
        for i = 1:N
            particles(i).weight = particles(i).weight / total_weight;
        end
    else
        % 权重全为0，重置
        for i = 1:N
            particles(i).weight = 1/N;
        end
    end
end

function expected_scan = simulate_scan_from_map(pose, occ_map, max_range, fov, num_beams, resolution, map_size)
% 从占据栅格地图模拟激光扫描
    
    x = pose(1);
    y = pose(2);
    theta = pose(3);
    
    angle_min = theta - fov/2;
    angle_max = theta + fov/2;
    angles = linspace(angle_min, angle_max, num_beams);
    
    grid_size = size(occ_map, 1);
    expected_scan = zeros(num_beams, 1);
    
    for i = 1:num_beams
        angle = angles(i);
        
        % 沿光线搜索
        range = 0;
        step = resolution;
        
        while range < max_range
            range = range + step;
            
            px = x + range * cos(angle);
            py = y + range * sin(angle);
            
            % 转换为栅格坐标
            % 当YDir='normal'时：imagesc的矩阵第1行对应y轴最小值(底部)
            % x -> 列索引: col = round(x/resolution) + 1
            % y -> 行索引: row = round(y/resolution) + 1
            gx = round(px / resolution) + 1;
            gy = round(py / resolution) + 1;
            
            % 检查边界
            if gx < 1 || gx > grid_size || gy < 1 || gy > grid_size
                range = max_range;
                break;
            end
            
            % 检查占据
            if occ_map(gy, gx) > 0  % log-odds > 0 表示占据
                break;
            end
        end
        
        expected_scan(i) = range;
    end
end

function likelihood = compute_scan_likelihood(scan1, scan2)
% 计算两个扫描的相似度
    
    % 使用高斯似然
    diff = scan1 - scan2;
    sigma = 0.5;  % 标准差
    
    % 忽略无效测量
    valid = (scan1 < 14.9) & (scan2 < 14.9);
    
    if sum(valid) == 0
        likelihood = 1e-10;
        return;
    end
    
    diff = diff(valid);
    
    % 高斯似然
    likelihood = exp(-sum(diff.^2) / (2 * sigma^2));
    likelihood = max(likelihood, 1e-10);
end

